/** ###################################################################
**     Filename  : Pid.C
**     Project   : CyberPark
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2011-7-31, 23:03
**     Contents  :
**         User source code
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/

/* MODULE Pid */
#include "Pid.h"

#define MAX_16 ((S16)0x7fff)
#define MIN_16 ((S16)0x8000)


static S32 L_sub(register S32 src_dst, register S32 src2)
{
	return src_dst - src2;
}

static S32 L_deposit_l(register S16 ssrc)
{
	return (S32)ssrc;
}

static S16 extract_l(register S32 lsrc)
{
	return (S16)lsrc;
}

static S32 L_mult(register S16 sinp1, register S16 sinp2)
{
	register S32 laccum;
	laccum = sinp1;
	laccum *= sinp2;
	return laccum;
}

static S32 L_add(register S32 src_dst, register S32 src2)
{
	return src_dst + src2;
}

S16 PID_controllerPIDtype1(S16 DesiredValue,S16 MeasuredValue,mc_sPIDparams *pParams)
{
  S32 ProportionalPortion, IntegralPortion, PIDoutput;
  S16 InputError;

  /*-------------------------------------------------------------------------------------------------*/
  /* Saturation mode must be set */
  /* InputError = sub(DesiredValue, MeasuredValue); */  /* input error */
  /*-------------------------------------------------------------------------------------------------*/
  /* input error calculation - 16bit range, with and without saturation mode  */
  PIDoutput = L_sub(L_deposit_l(DesiredValue),L_deposit_l(MeasuredValue)); /* input error - 32bit range */
  if(PIDoutput > MAX_16)               /* inpur error is greater than 0x00007fff = 32767  - 32bit range */
    InputError = MAX_16;               /* input error = max. positive 16 bit signed value */
  else
    if(PIDoutput < MIN_16)             /* input error is less than 0xffff7fff = -32768    - 32bit range */
      InputError = MIN_16;             /* input error = min. negative 16 bit signed value */
    else
      InputError = extract_l(PIDoutput); /* input error - 16bit range */

  /*-------------------------------------------------------------------------------------------------*/
  /* proportional portion calculation */
  ProportionalPortion = L_mult((pParams -> ProportionalGain), InputError) >> (U8)(pParams -> ProportionalGainScale + 1);

  /*-------------------------------------------------------------------------------------------------*/
  /* integral portion calculation */
  IntegralPortion = L_mult((pParams -> IntegralGain), InputError) >> (U8)(pParams -> IntegralGainScale + 1);

  /* integral portion in step k + integral portion in step k-1 */
  IntegralPortion = L_add(IntegralPortion, L_deposit_l(pParams -> IntegralPortionK_1));

  /* integral portion limitation */
  if(IntegralPortion > pParams -> PositivePIDLimit)
    pParams -> IntegralPortionK_1 = pParams -> PositivePIDLimit;
  else
    if(IntegralPortion < pParams -> NegativePIDLimit)
      pParams -> IntegralPortionK_1 =  pParams -> NegativePIDLimit;
    else
      pParams -> IntegralPortionK_1 =  extract_l(IntegralPortion);

  /*-------------------------------------------------------------------------------------------------*/
  /* derivative portion calculation */
  PIDoutput = L_sub(L_deposit_l(InputError),L_deposit_l(pParams -> InputErrorK_1)); /* [e(k) - e(k-1)] - 32bit range */
  pParams -> InputErrorK_1 = InputError; /* e(k-1) = e(k) */
  if(PIDoutput > MAX_16)               /* [e(k) - e(k-1)] is greater than 0x00007fff = 32767  - 32bit range */
    InputError = MAX_16;               /* [e(k) - e(k-1)] = max. positive 16 bit signed value - 16 bit range */
  else
    if(PIDoutput < MIN_16)             /* [e(k) - e(k-1)] is less than 0xffff7fff = -32768    - 32bit range */
      InputError = MIN_16;             /* [e(k) - e(k-1)] = min. negative 16 bit signed value - 16 bit range */
    else
      InputError = extract_l(PIDoutput); /* [e(k) - e(k-1)]    - 16bit range */

  /* drivative portion in step k - integer */
  PIDoutput = L_mult((pParams -> DerivativeGain), InputError) >> (U8)(pParams -> DerivativeGainScale + 1);

  /*-------------------------------------------------------------------------------------------------*/
  /* controller output calculation */
  PIDoutput = L_add(PIDoutput, ProportionalPortion); /* derivative portion + proportional portion */
  PIDoutput = L_add(PIDoutput, L_deposit_l(pParams -> IntegralPortionK_1)); /* + integral portion = controller output */

  /* controller output limitation */
  if(PIDoutput > pParams -> PositivePIDLimit)
    PIDoutput = pParams -> PositivePIDLimit;
  else
    if(PIDoutput < pParams -> NegativePIDLimit)
      PIDoutput =  pParams -> NegativePIDLimit;

  /*-------------------------------------------------------------------------------------------------*/
	    
  return (extract_l(PIDoutput));  /* controller output with limitation */
}




/* END Pid */
